package com.koc.robot.common;

import java.util.Arrays;

public class Matrix 
{
	private float e[] = new float[16];
	
	public Matrix()
	{
		setIdentity();
	}
	
	public Matrix(Matrix matrix)
	{
		if (null == matrix)
		{
			setIdentity();
		}
		else
		{
			System.arraycopy(matrix.e, 0, e, 0, e.length);
		}
	}
	
	public Matrix mult(Matrix m)
	{
		Matrix ret = new Matrix();
		
		ret.e[0] = (e[0]*m.e[0]) + (e[4]*m.e[1]) + (e[8]*m.e[2]) + (e[12]*m.e[3]);
		ret.e[4] = (e[0]*m.e[4]) + (e[4]*m.e[5]) + (e[8]*m.e[6]) + (e[12]*m.e[7]);
		ret.e[8] = (e[0]*m.e[8]) + (e[4]*m.e[9]) + (e[8]*m.e[10]) + (e[12]*m.e[11]);
		ret.e[12] = (e[0]*m.e[12]) + (e[4]*m.e[13]) + (e[8]*m.e[14]) + (e[12]*m.e[15]);

		ret.e[1] = (e[1]*m.e[0]) + (e[5]*m.e[1]) + (e[9]*m.e[2]) + (e[13]*m.e[3]);
		ret.e[5] = (e[1]*m.e[4]) + (e[5]*m.e[5]) + (e[9]*m.e[6]) + (e[13]*m.e[7]);
		ret.e[9] = (e[1]*m.e[8]) + (e[5]*m.e[9]) + (e[9]*m.e[10]) + (e[13]*m.e[11]);
		ret.e[13] = (e[1]*m.e[12]) + (e[5]*m.e[13]) + (e[9]*m.e[14]) + (e[13]*m.e[15]);

		ret.e[2] = (e[2]*m.e[0]) + (e[6]*m.e[1]) + (e[10]*m.e[2]) + (e[14]*m.e[3]);
		ret.e[6] = (e[2]*m.e[4]) + (e[6]*m.e[5]) + (e[10]*m.e[6]) + (e[14]*m.e[7]);
		ret.e[10] = (e[2]*m.e[8]) + (e[6]*m.e[9]) + (e[10]*m.e[10]) + (e[14]*m.e[11]);
		ret.e[14] = (e[2]*m.e[12]) + (e[6]*m.e[13]) + (e[10]*m.e[14]) + (e[14]*m.e[15]);

		ret.e[3] = (e[3]*m.e[0]) + (e[7]*m.e[1]) + (e[11]*m.e[2]) + (e[15]*m.e[3]);
		ret.e[7] = (e[3]*m.e[4]) + (e[7]*m.e[5]) + (e[11]*m.e[6]) + (e[15]*m.e[7]);
		ret.e[11] = (e[3]*m.e[8]) + (e[7]*m.e[9]) + (e[11]*m.e[10]) + (e[15]*m.e[11]);
		ret.e[15] = (e[3]*m.e[12]) + (e[7]*m.e[13]) + (e[11]*m.e[14]) + (e[15]*m.e[15]);
		
		return ret;
	}
	
	public void multLocal(Matrix m)
	{
		Matrix t = new Matrix(this);
		System.arraycopy(t.e, 0, e, 0, e.length);
	}
	
	public float[] getElements()
	{
		return e;
	}
	
	public void setIdentity()
	{
		Arrays.fill(e, 0.0f);
		e[0] = e[5] = e[10] = e[15] = 1.0f;
	}
	
	public void setTranslation(float x, float y)
	{
		setIdentity();
		e[12] = x;
		e[13] = y;
	}
	
	public void setRotation(float rad)
	{
		setIdentity();
		float c = (float)Math.cos(rad), s = (float)Math.sin(rad);
		e[0] = c; e[4] = -s;
		e[1] = s; e[5] = c;
	}
	
	public void setScale(float sx, float sy)
	{
		setIdentity();
		e[0] = sx;
		e[5] = sy;
	}
	
	public void setShear(float kx, float ky)
	{
		setIdentity();
		e[1] = ky;
		e[4] = kx;
	}
	
	public void setTransformation(float x, float y, float angle, float sx, float sy, float ox, float oy, float kx, float ky)
	{
		Arrays.fill(e, 0.0f);
		float c = (float)Math.cos(angle), s = (float)Math.sin(angle);
		
		e[10] = e[15] = 1.0f;
		e[0] = c * sx - ky * s * sy;
		e[1] = s * sx + ky * c * sy;
		e[4] = kx * c * sx - s * sy;
		e[5] = kx * s * sx + c * sy;
		e[12] = x - ox * e[0] - oy * e[4];
		e[13] = y - ox * e[1] - oy * e[5];
	}
	
	public void translate(float x, float y)
	{
		Matrix t = new Matrix();
		t.setTranslation(x,  y);
		this.multLocal(t);
	}
	
	public void rotate(float rad)
	{
		Matrix t = new Matrix();
		t.setRotation(rad);
		this.multLocal(t);
	}
	
	public void scale(float sx, float sy)
	{
		Matrix t = new Matrix();
		t.setScale(sx, sy);
		this.multLocal(t);
	}
	
	public void shear(float kx, float ky)
	{
		Matrix t = new Matrix();
		t.setShear(kx, ky);
		this.multLocal(t);
	}
	
	public void transform(Vector[] dst, Vector[] src)
	{
		assert(dst.length == src.length);
		
		for (int i = 0; i < src.length; i++)
		{
			float x = (e[0] * src[i].x) + (e[4] * src[i].y) + (0) + (e[12]);
			float y = (e[1] * src[i].x) + (e[5] * src[i].y) + (0) + (e[13]);
			
			dst[i].x = x;
			dst[i].y = y;
		}
	}
}
